using System;
using System.Threading;
using Microsoft.SPOT;
using SecretLabs.NETMF.Hardware.Netduino;
using NetduinoHappyHour.Behaviour;
using NetduinoHappyHour.Hardware;
using NetduinoHappyHour.Util;

namespace NetduinoHappyHour
{
    class NetduinoMain
    {
        public static void Main()
        {
            IRobotGateway gw = null;
            IExecutable behave = null;

            Thread.Sleep(3000);
            try
            {
                gw = RobotGatewayFactory.Instance();
                gw.BlinkShort();
            if (SystemConfig.Instance().UseStepperDriving)
            {
                new StepperSleep(true).Execute();
            }
            //CheckBattery checkBattery = new CheckBattery();
            //checkBattery.Execute();
            //DisplayMessage startupMessage = new DisplayMessage("Hello Salvador", "Battery: " + checkBattery.ToString());
            //checkBattery.Dispose();
            //DisplayMessage startupMessage = new DisplayMessage("Hello", "Salvador");
            //startupMessage.Execute();

                //behave = new ReadSensor();
                //behave = new Explore();
                //behave = new ExploreByUltrasound();
                //behave = new Interactive();
                //DriveCommandDC behave = new DriveCommandDC(20, 20, 5); SystemConfig.Instance().UseStepperDriving = false;
                //behave = new Path();
                behave = new BackForth();
                //behave = new TestToMeasure();
            }
            catch (Exception ex)
            {
                // TODO: testing !
                SystemStatus.Instance().AddMessage(ex + ex.Message + ex.StackTrace);
                new DisplayMessage("Exception", "Check SystemStatus").Execute();
                //gw.BlinkInfinite();
                if (gw != null)
                {
                    gw.BlinkShort();
                }
            }
            // the infinite loop is basic requirement for controler programming
            // alternatively its possible to use Thread.IsSleepMode(Timeout.Infinite) in junction with event handling
            while (true)
            {
                try
                {
                gw.BlinkShort();
                //gw.WaitForPinOrButton(Pins.GPIO_PIN_D8);

                    behave.Execute();
                }
                catch (Exception ex)
                {
                    // TODO: testing !
                    SystemStatus.Instance().AddMessage(ex + ex.Message + ex.StackTrace);
                    new DisplayMessage("Exception", "Check SystemStatus").Execute();
                    if (gw != null)
                    {
                    gw.BlinkShort();
                }
                }
                // -1 causes Thread never to wake up again
                Thread.Sleep(3000);
                // Thread.Sleep(-1);

                // might not be reached, it depends on implementation ov Behaviours ExecuteDriveCommand method
                // gw.BlinkShort();
            }
        }
    }
}
